SUBROUTINE integr_rk87 (mjdo, yo, step, mjdn, yn, ey)


! ----------------------------------------------------------------------
! SUBROUTINE: integr_rk87.f03
! ----------------------------------------------------------------------
! Purpose:
!  Runge-Kutta Numerical integration method: RK8(7)-13 by Prince and Dormand (1981) 
!  RK8(7)-13 method is applied here for satellite orbit integration
! ----------------------------------------------------------------------
! Input arguments:
! - MJDo: 		Initial epoch's Modified Julian Day (MJD) number (including the fraction of the day)
! - yo:			Initial State vector: Position (m) and Velocity (m/sec) vectors
! - step:		Numerical Integration stepsize (sec)
!
! Output arguments
! - MJD: 		Next epoch's (to+h) Modified Julian Day (MJD) number (including the fraction of the day)
! - y:			State vector at next epoch: Position (m) and Velocity (m/sec) vectors
! ----------------------------------------------------------------------
! Author :	Dr. Thomas Papanikolaou, Cooperative Research Centre for Spatial Information, Australia
! Created:	19 October 2017
! ----------------------------------------------------------------------

	  
	  
      USE mdl_precision
      USE mdl_num
      IMPLICIT NONE


! ----------------------------------------------------------------------
! Dummy arguments declaration
! ----------------------------------------------------------------------
! IN
      REAL (KIND = prec_d), INTENT(IN), DIMENSION(6) :: yo
      REAL (KIND = prec_d), INTENT(IN) :: mjdo
      REAL (KIND = prec_d), INTENT(IN) :: step
! ----------------------------------------------------------------------
! OUT
      REAL (KIND = prec_d), INTENT(OUT), DIMENSION(6) :: yn, ey
      REAL (KIND = prec_d), INTENT(OUT) :: mjdn
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Local variables declaration
! ----------------------------------------------------------------------
      REAL (KIND = prec_d) :: h, to, mjd_to
      REAL (KIND = prec_d), DIMENSION(3) :: ro, vo
      INTEGER (KIND = prec_int4) :: s_stages, i, j
      REAL (KIND = prec_d) :: ci(13)
      REAL (KIND = prec_d) :: bp(13) 
      REAL (KIND = prec_d) :: bq(13) 
      REAL (KIND = prec_d) :: aij(13,12) 
      REAL (KIND = prec_d), DIMENSION(6) :: sum_ak 
      REAL (KIND = prec_d), DIMENSION(6,13) :: ki 
      REAL (KIND = prec_d) :: fx, fy, fz
      REAL (KIND = prec_d) :: ti, mjd_ti
      REAL (KIND = prec_d), DIMENSION(6) :: yi
      REAL (KIND = prec_d), DIMENSION(3) :: ri, vi
      REAL (KIND = prec_d) :: Fq(6), Fp(6)
      REAL (KIND = prec_d) :: yq(6), yp(6) 
! ----------------------------------------------------------------------	  



! ----------------------------------------------------------------------	  
! Integration step
h = step
! ----------------------------------------------------------------------	  


! ----------------------------------------------------------------------	  
! Initial Conditions
! ----------------------------------------------------------------------	  
! Initial epoch
! MJD
mjd_to = mjdo
! Seconds at MJDo since 00h
to = (mjd_to - INT(mjd_to)) * (24.0D0 * 3600.D0)

! Initial State vector at epoch to: Position and Velocity vectors
ro = yo(1:3)
vo = yo(4:6)
! ----------------------------------------------------------------------	



! ----------------------------------------------------------------------	
! Coefficients ci, aij, bi 
! ----------------------------------------------------------------------	

! ----------------------------------------------------------------------	
! ci coefficients, i=0,....,s
ci(1:13) = (/ 0.D0, 1.D0/18.D0, 1.D0/12.D0, 1.D0/8.D0, 5.D0/16.D0, 3.D0/8.D0, 59.D0/400.D0, 93.D0/200.D0, & 
            5490023248.D0/9719169821.D0, 13.D0/20.D0, 1201146811.D0/1299019798.D0, 1.D0, 1.D0 /)
! ----------------------------------------------------------------------	
	
	
! ----------------------------------------------------------------------
! aij coefficients, i=1,....,s, j=0,...,s-1
! ----------------------------------------------------------------------
aij(1,1:12) = (/ 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)
aij(2,1:12) = (/ 1.D0/18.D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)
aij(3,1:12) = (/ 1.D0/48.D0, 1.D0/16.D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)
aij(4,1:12) = (/ 1.D0/32.D0, 0.0D0, 3.D0/32.D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)
aij(5,1:12) = (/ 5.D0/16.D0, 0.0D0, -75.D0/64.D0, 75.D0/64.D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)
aij(6,1:12) = (/ 3.D0/80.D0, 0.0D0, 0.0D0, 3.D0/16.D0, 3.D0/20.D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)

aij(7,1:12) = (/ 29443841.D0/614563906.D0, 0.0D0, 0.0D0, 77736538.D0/692538347.D0, -28693883.0D0/1125000000.D0, & 
               23124283.0D0/1800000000.D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)

aij(8,1:12) = (/ 16016141.D0/946692911.D0, 0.0D0, 0.0D0, 61564180.D0/158732637.D0, 22789713.D0/633445777.D0, &
               545815736.0D0/2771057229.D0, -180193667.D0/1043307555.D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)

aij(9,1:12) = (/ 39632708.D0/573591083.D0, 0.0D0, 0.0D0, -433636366.D0/683701615.D0, -421739975.D0/2616292301.D0, &
               100302831.D0/723423059.D0, 790204164.D0/839813087.D0, 800635310.D0/3783071287.D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)

aij(10,1:12)= (/ 246121993.D0/1340847787.D0, 0.0D0, 0.0D0, -37695042795.D0/15268766246.D0, -309121744.D0/1061227803.D0, &
              -12992083.D0/490766935.D0, 6005943493.D0/2108947869.D0, 393006217.D0/1396673457.D0, 123872331.D0/1001029789.D0, &
			  0.0D0, 0.0D0, 0.0D0 /)

aij(11,1:12)= (/ -1028468189.D0/846180014.D0, 0.0D0, 0.0D0, 8478235783.D0/508512852.D0, 1311729495.D0/1432422823.D0, &
               -10304129995.D0/1701304382.D0, -48777925059.D0/3047939560.D0, 15336726248.D0/1032824649.D0,  &
			   -45442868181.D0/3398467696.D0, 3065993473.D0/597172653.D0, 0.0D0, 0.0D0 /)
  
aij(12,1:12) = (/ 185892177.D0/718116043.D0, 0.0D0, 0.0D0, -3185094517.D0/667107341.D0, -477755414.D0/1098053517.D0, &
                  -703635378.D0/230739211.D0, 5731566787.D0/1027545527.D0 , 5232866602.D0/850066563.D0, &
				  -4093664535.D0/808688257.D0, 3962137247.D0/1805957418.D0, 65686358.D0/487910083.D0, 0.0D0 /)
			  
aij(13,1:12) = (/ 403863854.D0/491063109.D0, 0.0D0, 0.0D0, -5068492393.D0/434740067.D0, -411421997.D0/543043805.D0, &
                  652783627.D0/914296604.D0, 11173962825.D0/925320556.D0, -13158990841.D0/6184727034.D0, &
				  3936647629.D0/1978049680.D0, -160528059.D0/685178525.D0, 248638103.D0/1413531060.D0, 0.0D0 /)
! ----------------------------------------------------------------------
	

! ----------------------------------------------------------------------
! Coefficients bp, order p,  i=0,....,s
bp(1:13) = (/ 13451932.D0/455176623.D0, 0.D0, 0.D0, 0.D0, 0.D0, -808719846.D0/976000145.D0, 1757004468.D0/5645159321.D0, &
              656045339.D0/265891186.D0, -3867574721.D0/1518517206.D0, 465885868.D0/322736535.D0, 53011238.D0/667516719.D0, &
		      2.D0/45.D0, 0.D0 /)
		
! Coefficients bq, order q=p+1,  i=0,....,s
!bq(1:13) = (/ 14005451.D0/335480064.D0, 0.D0, 0.D0, 0.D0, 0.D0, &
!             -59238493.D0/1068277825.D0, 181606767.D0/758867731.D0, &
!              561292985.D0/797845732.D0, -1041891430.D0/1371343529.D0, 760417239.D0/1151165299.D0, 118820643.D0/751138087.D0 &
!			 -528747749.D0/2220607170.D0, 1.D0/4.D0 /)

bq(1) = 14005451.D0/335480064.D0
bq(2:5) = (/ 0.D0, 0.D0, 0.D0, 0.D0 /)
bq(6:7) = (/ -59238493.D0/1068277825.D0, 181606767.D0/758867731.D0 /)
bq(8:11) = (/ 561292985.D0/797845732.D0, -1041891430.D0/1371343529.D0, 760417239.D0/1151165299.D0, 118820643.D0/751138087.D0 /)
bq(12:13) = (/ -528747749.D0/2220607170.D0, 1.D0/4.D0 /)
! ----------------------------------------------------------------------


! ----------------------------------------------------------------------	  
! Computation of integration coefficients (k)
! ----------------------------------------------------------------------	
! s-stage Function evaluations k(s), s=0..to...
s_stages = 13


! k1  
! Force model acceleration components
Call force_sum(mjd_to, ro, vo, fx, fy, fz)
! ki(:,1) = f(to,yo)
ki(:,1) = (/ vo(1), vo(2), vo(3), fx, fy, fz /)

! ki  
Do i = 2 , s_stages

ti = to + ci(i) * h

! MJD (in days) at ti including the fraction of the day
mjd_ti = INT(mjd_to) + ti / (24.D0 * 3600.D0)


! SUM(aij*ki)
sum_ak = (/ 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)
Do j = 1 , i-1

sum_ak = sum_ak + aij(i,j) * ki(:,j)

End Do


yi = yo + h * sum_ak

! Position and Velocity vectors
ri = yi(1:3)
vi = yi(4:6)

! Force model acceleration components
Call force_sum(mjd_ti, ri, vi, fx, fy, fz)
!ki(:,i) = f(ti,yi)
ki(:,i) = (/ vi(1), vi(2), vi(3), fx, fy, fz /)

End Do
! ----------------------------------------------------------------------	  


! ----------------------------------------------------------------------	  
! Increment function
Fp = (/ 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)
Fq = (/ 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0, 0.0D0 /)
!Do i = 1 , s_stages
Do i = 1 , s_stages

Fp = Fp + bp(i) * ki(:,i)
Fq = Fq + bq(i) * ki(:,i)

End Do
! ----------------------------------------------------------------------	  


! ----------------------------------------------------------------------	
! Solution of the numerical integration
! ----------------------------------------------------------------------	
! Order q=8
yq = yo + h * Fq

! Order p=7
yp = yo + h * Fp
! ----------------------------------------------------------------------	


! ----------------------------------------------------------------------	
! Local truncation error - Stepsize control
ey = abs(yq - yp)
! ----------------------------------------------------------------------	
!print *,"ey", ey

! ----------------------------------------------------------------------	
! Next Epoch solution

! MJD (in days) including the fraction of the day
mjdn = mjdo + step / (24.0D0 * 3600.D0) 

! State vector at the next epoch: 
!yn = yp !order solution p=7 
yn = yq !order solution q(p+1)=8 
! ----------------------------------------------------------------------	




END SUBROUTINE
